#include "common.h"
#include "drone.h"
#include "car.h"
#include "cooperate_locate.h"
#include <iostream>

/* int main2() {
    // 测试数据
    double pixel_x, pixel_y;
    std::cout << "请输入像素坐标：";
    std::cin >> pixel_x >> pixel_y;
    Vector3d pixel(pixel_x, pixel_y, 1); // 像素坐标
    Vector3d drone_pos(0, 0, 1.03); // 无人机全局位置 (x, y, z)
    Vector3d camera_pos(0, 0, 0); // 相机在无人机本体坐标系中的安装偏移量

    // 创建无人机旋转矩阵（假设无人机无旋转）
    Matrix3x3 drone_rot = drone_coord::create_rotation_matrix(0, 0, 0);

    // 创建相机安装旋转矩阵（向下45度）
    Matrix3x3 camera_rot = drone_coord::create_rotation_matrix(0, -M_PI / 4, 0);

    // 设置相机内参
    drone_coord::CameraIntrinsics intrinsics;

    // 计算相机全局位置
    Vector3d global_pos = drone_coord::get_global_pos(drone_pos, camera_pos, drone_rot);
    std::cout << "相机全局位置: (" << global_pos.x << ", " << global_pos.y << ", " << global_pos.z << ")" << std::endl;

    // 计算像素到全局坐标的转换
    Vector3d result;
    Vector3d ray_drone; // 用于存储射线方向
    double t;           // 用于存储计算得到的t值
    bool success = drone_coord::pixel_to_global(pixel, global_pos, drone_rot, camera_rot, intrinsics, result, ray_drone, t);

    if (success) {
        std::cout << "射线方向: (" << ray_drone.x << ", " << ray_drone.y << ", " << ray_drone.z << ")" << std::endl;
        std::cout << "t值: " << t << std::endl;
        std::cout << "目标物体的全局坐标为: (" << result.x << ", " << result.y << ", " << result.z << ")" << std::endl;
    } else {
        std::cout << "未能找到地面交点。" << std::endl;
    }

    return 0;
} */
void compare_wc(Vector3d& v1, Vector3d& v2){
    // 以v1的经纬度为基准建立ENU坐标系
    ENUConverter converter(v1.x, v1.y);
    
    // 转换v1和v2到ENU坐标
    double x1, y1, x2, y2;
    converter.Forward(v1.x, v1.y, x1, y1);  // v1在自身坐标系中的坐标应为(0,0)
    converter.Forward(v2.x, v2.y, x2, y2);
    
    // 计算相对误差（米）
    double error_x = x2 - x1;  // 在ENU坐标系中，x2应为0
    double error_y = y2 - y1;  // 在ENU坐标系中，y2应为0
    
    std::cout << "经度误差: " << error_x << " 米" << std::endl;
    std::cout << "纬度误差: " << error_y << " 米" << std::endl;
    std::cout << "总误差: " << sqrt(error_x*error_x + error_y*error_y) << " 米" << std::endl;
    std::cout << std::endl << std::endl;
}
void test(){
    // 测试航向角、 偏离角的方向和值
    Vehicle car1 = {0, 0, M_PI / 2, 0};
    Vehicle car2 = {10, 0, M_PI / 2, -M_PI / 6};


    std::cout << std::fixed << std::setprecision(2);
    std::cout << "car1 enu:(" << car1.x << ", " << car1.y << ")"
              << std::endl;

    std::cout << std::fixed << std::setprecision(2);
    std::cout << "car2 enu:(" << car2.x << ", " << car2.y << ")"
              << std::endl;
    
    Vector3d obj;
    if (calculateObjectPosition(car1, car2, obj.x, obj.y))
        std::cout << "obj enu from car:(" << obj.x << ", " << obj.y << ")"
                  << std::endl;
    else
        std::cout << "ERROR" << std::endl;
}

void test2(){
    // 测试弧度和角度的转换
    Vehicle car1 = {0, 0, degreesToRadians(90), degreesToRadians(0)};
    Vehicle car2 = {10, 0, degreesToRadians(90), -degreesToRadians(30)};


    std::cout << std::fixed << std::setprecision(2);
    std::cout << "car1 enu:(" << car1.x << ", " << car1.y << ")"
              << std::endl;

    std::cout << std::fixed << std::setprecision(2);
    std::cout << "car2 enu:(" << car2.x << ", " << car2.y << ")"
              << std::endl;
    
    Vector3d obj;
    if (calculateObjectPosition(car1, car2, obj.x, obj.y))
        std::cout << "obj enu from car:(" << obj.x << ", " << obj.y << ")"
                  << std::endl;
    else
        std::cout << "ERROR" << std::endl;
}

void test3(){
    // 测试双车定位，找出问题：射线没相交
    // VehicleWgs84 car1 = {118.8579154, 32.0266936, degreesToRadians(97.96), degreesToRadians(18.68)};
    
    VehicleWgs84 car1 = {118.8579383, 32.0266940, degreesToRadians(29.16), degreesToRadians(8.40)};
    VehicleWgs84 car2 = {118.8578912, 32.0266808, degreesToRadians(5.36), degreesToRadians(4.30)};
    Vector3d obj;
    bool flag = true;
    obj = get_obj_enu_from_car(car1, car2, flag);
    std::cout << "obj enu from car:(" << obj.x << ", " << obj.y << ")"
              << std::endl;
    obj_from_enu2wgs84(car1, obj);
}

void test4(){
    // 测试相机偏移
    
    VehicleWgs84 car1 = {118.8579000, 32.0266000, degreesToRadians(0), degreesToRadians(0)};
    VehicleWgs84 car2 = {118.8580059, 32.0266000, degreesToRadians(-90), degreesToRadians(-60)};
    Vector3d obj;
    bool flag = true;
    obj = get_obj_enu_from_car(car1, car2, flag);
    if(flag == false) 
        std::cout << "car locate error" << std::endl;
    else obj_from_enu2wgs84(car1, obj);
}

void test01_drone()
{
    Vector3d drone_wgs84(118.8563362,32.0266519, 71);
    Vector3d camera_pos(0.10, 0, 0);
    Vector3d pixel(130, 372.5, 1); // 在图片中心
    auto drone_rot = drone_coord::create_rotation_matrix(0.7, 0, 0);
    auto camera_rot = drone_coord::create_rotation_matrix(0, -M_PI/ 4, 0);

    Vector3d obj_enu;
    bool flag = true;
    obj_enu = get_obj_enu_from_drone(drone_wgs84, camera_pos, pixel, drone_rot, camera_rot, flag);

    if(flag){
        // 测试转经纬度
        VehicleWgs84 base(drone_wgs84.x, drone_wgs84.y, 0, 0);
        Vector3d obj_wgs84;
        obj_wgs84 = obj_from_enu2wgs84(base, obj_enu);


        Vector3d v1(118.8563330,32.0271650, 0);
        compare_wc(v1, obj_wgs84);
    }else{
        std::cout << "drone locate ERROR" << std::endl;
    }
}

int main(int argc, char const *argv[])
{
    test01_drone();
}
